#include <tf/transform_broadcaster.h>

#define TWO_PI 6.28318531

class Navigation
{
public:
	// Initialization the fetches data from parameter server
	Navigation(float startX, float startY, float startYaw,
	           unsigned char leftStop, unsigned char rightStop,
	           unsigned char leftLowRange, unsigned char rightLowRange,
	           unsigned char leftHighRange, unsigned char rightHighRange);

	~Navigation();

	// Callbacks that store incoming navigation data
	void lineCallback( const geometry_msgs::PointConstPtr& lineParams );
	void waypointCallback( const geometry_msgs::PointConstPtr& goal );

	// Integrates motion to estimate current position
	void updatePosition( void );
	
	// Calculates the current wheel speeds based upon 
	// the PID controller, most recent position data,
	// and calibration
	unsigned char getLeftWheelSpeed( void );
	unsigned char getRightWheelSpeed( void );

	const tf::Vector3& getPosition() { return position; };
	float getYaw() { return yaw; };

private:
	// Functions used to determine motion components
	float getLinearSpeed();
	float getAngularSpeed();

	// Utility class to do angle comparisons
	bool angleInLeeway(float angle, float reference);
	float getDistance( void );

	// Positioning Information
	geometry_msgs::Point waypoint; // Target position
	tf::Vector3 position; // Current position
	float yaw; // Current rotation
	float lineOffset; // Current offset from line
	float referenceAngle; // Angle of tape
	float targetAngle; // Angle the seeker is trying to achieve
	ros::Time lastUpdate; // Last time position was updated

	// Dead-zones
	float angularLee;

	// Motor control data
	unsigned char leftZero; // Calibration point for left wheel
	unsigned char rightZero; // Calibration point for right wheel
	float maxLinearSpeed;
	float maxAngularSpeed;
	unsigned char leftMaxValue;
	unsigned char rightMaxValue;
	unsigned char leftMinValue;
	unsigned char rightMinValue;
	float deadZoneDistance;
};
